#include "ulti.h"


using namespace cv;


bool findPerspectiveMatrix(CvCapture* cap,int ids[4],CvMat* map_matrix,int width,int height){
	IplImage * img;
	vector<shared_ptr<Marker>> markers;
	bool found=false;
	CvPoint2D32f from[4];
	for(int i=0;i<MAX_TRIAL&&(!found);i++){
		img=cvQueryFrame(cap);
		markers.clear();
		detectMarker(img,markers);
		int cornersfound=0;

		if(markers.size()>=4)
		{
			for(int j=0;j<4;j++)
			{
				for(int k=0;k<markers.size();k++)
				{
					if(markers.at(k)->id==ids[j])
					{
						cornersfound++;
						from[j]=cvPoint2D32f(markers.at(k)->x,markers.at(k)->y);
						break;
					}
				}
			}
			if(cornersfound==4)found=true;
		}
		Sleep(DETECT_INTERVAL);
	}
	if(found){
		CvPoint2D32f to[4]={cvPoint2D32f(0,0),cvPoint2D32f(width,0),cvPoint2D32f(0,height),cvPoint2D32f(width,height)};

		cvGetPerspectiveTransform(from,to,map_matrix);
		cout<<"reference corners found"<<endl;
	}

	return true;
};

CvPoint perspectiveTransform_1pnt(CvPoint src,CvMat* matrix){
	CvMat* source=cvCreateMat(1,1,CV_32FC2);
	CvMat* destination=cvCreateMat(1,1,CV_32FC2);
	cvSet1D(source,0,cvScalar(src.x,src.y));
	cvPerspectiveTransform(source,destination,matrix);
	return cvPoint(cvGet1D(destination,0).val[0],cvGet1D(destination,0).val[1]);
}

float calculateAngle(CvPoint ref_head,CvPoint ref_tail,CvPoint obj_head,CvPoint obj_tail){
	float ref_angle=atan2(float(ref_tail.y-ref_head.y), float(ref_tail.x-ref_head.x)) * 180.0 / CV_PI; 
	float object_angle=atan2(float(obj_tail.y-obj_head.y),float(obj_tail.x-obj_head.x)) * 180.0 / CV_PI; 
	float angle_diff=object_angle-ref_angle;
	if(angle_diff<0)angle_diff+=360;
	return angle_diff;
}

bool findAMarker(IplImage* img, int markerid,shared_ptr<Marker>& marker){
	vector<shared_ptr<Marker>> markers;
	detectMarker(img,markers);
	bool found=false;
	for(int i=0;i<markers.size();i++)
	{
		if(markers.at(i)->id==markerid){
			found=true;
			marker=markers.at(i);
		}
	}
	return found;
}

void howToAPoint(CvPoint pointFrom,CvPoint pointTo,CvPoint obj_head,CvPoint obj_tail, float& result_angle,float& result_distance){
	result_distance=sqrt(pow(float(pointTo.x-pointFrom.x),2)+pow(float(pointTo.y-pointFrom.y),2));
	float angle=calculateAngle(obj_tail,obj_head,pointFrom,pointTo);
	result_angle=angle;
}

void goToAPoint(float angle,float distance)
{
	if(distance<100&&angle<180&&angle>100){
		TurnLeft((180-angle)/90.0);
		Back(distance/100.0);
	}
	else if(distance<100&&angle>=180&&angle<260){
		TurnRight((angle+180-360)/90.0);
		Back(distance/100.0);
	}
	else{
		if(angle>180){
			TurnLeft((360-angle)/90.0);
		}
		else
		{
			TurnRight(angle/90.0);
		}
		Forward(distance/100.0);
	}
}

void trunAngle(float angle){
	if(angle>180){
		TurnLeft((360-angle)/90.0);
	}
	else
	{
		TurnRight(angle/90.0);
	}
}



